#include <chassis_controller/current_monitor.h>
#include <boost/bind.hpp>



namespace chassis_controller
{
  namespace bacc = boost::accumulators;

  CurrentMonitor::CurrentMonitor()
  {
  }
  void CurrentMonitor::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node,
      std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVEvent>> event_pub,
      const rclcpp::Time &time, 
      std::vector<Spivot_value> &pivot_value){
      
      node_handle_ = node;
      event_pub_ = event_pub;
      timestamp_ = time;
      // init wheel pos
      pivot_num_ = pivot_value.size();
      //current_accu_vec_.resize(pivot_num_);
      current_vec_.resize(pivot_num_);
      current_mean_vec_.resize(pivot_num_);
      
      for (int i = 0; i < pivot_num_; i++)
      {

      }
  }

  void CurrentMonitor::update(std::vector<Spivot_value> &pivot_value, 
      std::vector<Spivot_param> &pivot_param,
      agv_msgs::msg::AGVEvent& event_msg,
      bool standstill,
      bool reset_button, 
      monitor_param &param)
  {
    reset_button_ = reset_button;
    if(standstill){
      standstill_time_ = node_handle_->now().seconds();
      for (int i = 0; i < pivot_num_; i++){
        current_mean_vec_[i].clear();
      }
    } 

    for (int i = 0; i < pivot_num_; i++){
      if (pivot_param[i].pivot_type == DIFFWHEEL){
        pivot_value[i].current = (abs(pivot_value[i].joint_value[0].current) + abs(pivot_value[i].joint_value[1].current))/2.0;
      }
      else if (pivot_param[i].pivot_type == STEERWHEEL){
        pivot_value[i].current = abs(pivot_value[i].current);
      }
      current_vec_[i] = CurrentMonitor::meanFilter(pivot_value[i].current,current_mean_vec_[i],standstill_time_,param.current_monitor_time);
      RCLCPP_INFO_STREAM_THROTTLE(node_handle_->get_logger(), *(node_handle_->get_clock()),
        500, "[CURRENT_MONITOR]" << pivot_param[i].name << "current is: "<<current_vec_[i]);
      //current_accu_vec_[i](pivot_value[i].current);
      //current_vec_[i] = bacc::rolling_mean(current_accu_vec_[i]);   
    }


    check_ready_ = (node_handle_->now().seconds() - standstill_time_) > param.current_monitor_time;

    if (check_ready_){
      double sum=0;
      for (int i = 0; i < pivot_num_; i++){
        sum += current_vec_[i];
      }
      double average_current = sum / pivot_num_;
      RCLCPP_INFO_STREAM_THROTTLE(node_handle_->get_logger(), *(node_handle_->get_clock()),
        500,"[CURRENT_MONITOR] average current is: "<<average_current);
      if ((pivot_num_ == 2) && (abs(current_vec_[0] - current_vec_[1]) > ((param.imbalance_threshold - 1.0) * average_current))){
        event_msg.ecode = agv_msgs::msg::AGVEvent::CURRENT_IMBALANCED;
        event_msg.description = "current imbalanced , over "+std::to_string(param.imbalance_threshold) +" times";
      }
      if (pivot_num_ > 2){
        for (int i = 0; i < pivot_num_; i++){
          double diff_current = current_vec_[i] - average_current;
          if (abs(diff_current)> ((param.imbalance_threshold - 1.0) * average_current)) {
            event_msg.ecode = agv_msgs::msg::AGVEvent::CURRENT_IMBALANCED;
            event_msg.description = pivot_param[i].name + 
            " current abnormal!!!"+" average current is: "+std::to_string(average_current) 
            +pivot_param[i].name +" current is: " + std::to_string(current_vec_[i]);
            break;  
          }
        }
      }
    }
    // pubError();
  }

  // void CurrentMonitor::setRollingWindowSize(size_t rolling_window_size)
  // {
  //   rolling_window_size_ = rolling_window_size;
  //   resetAccumulators();
  // }

  // void CurrentMonitor::resetAccumulators()
  // {
  //   for (int i = 0; i < pivot_num_; ++i){
  //     current_accu_vec_[i] = RollingMean(RollingWindow::window_size = rolling_window_size_);
  //   }
  // }
  double CurrentMonitor::meanFilter(double input ,std::vector<double> &vector,double start_time,double time_Window){
    vector.push_back(input);
    double sum =0.0;
    if ((node_handle_->now().seconds() - start_time) > time_Window){
      vector.erase(vector.begin());
    }
    for(auto iter : vector){
      sum += iter;
    }
    
    if (!vector.empty()){
      //sum = std::accumulate(std::begin(vector), std::end(vector), 0);
      double mean = static_cast<double>(sum)/vector.size();
      //RCLCPP_INFO_STREAM_THROTTLE(0.5, "[meanFilter]size,sum,mean:" <<vector.size()<<","<<sum<<","<<mean);
      return mean;
    }
    else 
    return 0.0;
  }

// void CurrentMonitor::pubError()
// {
//   if (reset_button_)
//   {
//     event_msg_.ecode = 0;
//   }

//   static uint32_t seq = 0;
//   rclcpp::Time stamptmp = rclcpp::Time::now();

//   event_msg_.header.seq = seq++;
//   event_msg_.header.stamp = stamptmp;
//   event_msg_.header.frame_id = ros::this_node::getName();
//   if (event_msg_.ecode == 0)
//   {
//     event_msg_.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
//     event_msg_.type = agv_msgs::msg::AGVEvent::INFO;
//     event_msg_.action = agv_msgs::msg::AGVEvent::NONE;
//   }

//   if (event_msg_.ecode != 0)
//   {
//     event_msg_.eventTime = stamptmp.toSec();
//     event_msg_.action = agv_msgs::msg::AGVEvent::STOP;
//     event_msg_.type = agv_msgs::msg::AGVEvent::ALARM;
//     event_msg_.ackReq = true;
//   }
//   if (event_msg_.ecode == 0 && (rclcpp::Time::now().toSec() - event_pub_time_ > 1.0) || event_msg_.ecode != 0)
//   {
//     event_pub_.publish(event_msg_);
//     event_pub_time_ = rclcpp::Time::now().toSec();
//   }
// }


} // namespace agv_tricycle_controller
